#ifndef RANSACBASE_H
#define RANSACBASE_H

#include <vector>

template <class SampleType> class RansacBase
{
 
 public:
	 /**********************************************/
	 RansacBase(int num_samples_for_solution, int solution_size):
		_num_samples_for_solution(num_samples_for_solution),
		_solution_size(solution_size),
		_inlier_threshold(1.0),
		_num_iterations(100)
	   {};
	/**********************************************/
	~RansacBase() {};

	/**********************************************/
	void setSamples(std::vector<SampleType>& samples)
	{
		_samples = samples;
	};
	
	/**********************************************/
	void setInlierThreshold(double inlier_threshold)
	{
		_inlier_threshold = inlier_threshold;
	};

	/**********************************************/
	void setNumIterations(int num_iterations)
	{
		_num_iterations = num_iterations;
	};

	/**********************************************/
	double run(std::vector<double>& solution, std::vector<int>& inliers)
	{
		assert(_samples.size() > 0);

		const int num_samples = _samples.size();
		_max_num_inliners = 0;

		std::vector<SampleType> samples(_num_samples_for_solution);
		for (int i=0; i < _num_iterations; ++i)
		{
			// Define the random samples
			for (int r=0; r < _num_samples_for_solution; ++r)
			{
				int index = rand() & (num_samples-1);
				samples[r] = _samples[index];
			}
			
			// Compute solution with the random samples
			std::vector<double> solution_local = computeSolution(samples);

			// Evaluate the solution
			std::vector<int> inliers_local;
			double num_inliners = evaluateSolution(solution_local, inliers_local);

			// Keep the best solution found so far
			if (num_inliners > _max_num_inliners)
			{
				_max_num_inliners = num_inliners;
				_best_solution = solution_local;
				_best_inliers = inliers_local;
			}
		}
		
		solution = _best_solution;
		inliers = _best_inliers;
		return _max_num_inliners;
	};

protected:	
	/**********************************************/
	virtual std::vector<double> computeSolution(std::vector<SampleType>& samples) = 0;

	/**********************************************/
	virtual double evaluateSolution(std::vector<double>& solution, std::vector<int>& inliers) = 0;
 
	// The number of samples required to derive a solution
	int _num_samples_for_solution;
	// The solution size (ie number of doubles needed)
	int _solution_size;

	// The complete set of samples to derive the optimal solution
	std::vector<SampleType> _samples;
	
	// Threshold to define inliers
	double _inlier_threshold;
	// The number of iterations performed
	int _num_iterations;
	
	// The max number of inliers
	double _max_num_inliners;
	// The inliers corresponding to the best solution
	std::vector<int> _best_inliers;
	// The best solution
	std::vector<double> _best_solution;
};

 #endif // RANSACBASE_H